/*
 ------------------------------------------------------------------------------
 Copyright (C) 2013 Eternal Games.

 This file is part of the GLQuake source code.

 The GLQuake source code is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or (at your
 option) any later version.

 The GLQuake source code is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 more details.

 You should have received a copy of the GNU General Public License along with
 the GLQuake source code; if not, write to the Free Software Foundation, Inc.,
 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 ------------------------------------------------------------------------------
*/


//
// cg_predict.cpp - local prediction of player movement
//


#include "cg_local.h"


static entity_t *	cg_predictEntities[MAX_SNAPSHOT_ENTITIES];
static int			cg_numPredictEntities;


/*
 ==============================================================================

 ENTITY STATE PREDICTION

 ==============================================================================
*/


/*
 ==================
 CG_GetBindTransform
 ==================
*/
static bool CG_GetBindTransform (entity_t *entity, glqVec3 &baseOrigin, glqMat3 &baseAxis, glqVec3 &masterOrigin, glqMat3 &masterAxis){

	entity_t		*master;
	entityState_t	*baseline;

	if (entity->state->bindMaster == ENTITYNUM_NONE)
		return false;

	master = &cgs.entities[entity->state->bindMaster];
	baseline = &cgs.baselines[entity->state->bindMaster];

	// Get the base transform
	baseOrigin = baseline->origin.base;
	baseAxis = baseline->angles.base.ToMat3();

	// Get the master transform
	masterOrigin = master->snapshotOrigin;
	masterAxis = master->snapshotAxis;
	
	return true;
}

/*
 ==================
 CG_CalcPositionAndOrientation
 ==================
*/
static void CG_CalcPositionAndOrientation (entity_t *entity){

	glqVec3	baseOrigin, masterOrigin;
	glqMat3	baseAxis, masterAxis;

	// Calculate position and orientation
	entity->snapshotOrigin = entity->state->origin.Evaluate(cg.snapshot->time, cgs.gravity);
	entity->snapshotAngles = entity->state->angles.Evaluate(cg.snapshot->time, cgs.gravity);

	entity->snapshotAngles.Normalize360();
	entity->snapshotAxis = entity->snapshotAngles.ToMat3();

	// Adjust for binding if necessary
	if (CG_GetBindTransform(entity, baseOrigin, baseAxis, masterOrigin, masterAxis)){
		// Adjust position
		baseAxis.ProjectVector(entity->snapshotOrigin - baseOrigin, entity->snapshotOrigin);

		entity->snapshotOrigin = masterOrigin + masterAxis * entity->snapshotOrigin;

		// Adjust orientation if desired
		if (entity->state->bindOrientated){
			entity->snapshotAxis = entity->snapshotAxis.TransposeMultiply(baseAxis) * masterAxis;
			entity->snapshotAngles = entity->snapshotAxis.ToAngles();
		}
	}
}

/*
 ==================
 
 ==================
*/
void CG_SetupPredictEntities (){

	entity_t	*entity;
	int			i;

	if (!cg.setupPrediction)
		return;
	cg.setupPrediction = false;

	// Clear prediction entities
	cg_numPredictEntities = 0;

	// If we're interpolating, we don't need to set up prediction entities
	if (cgs.demoPlaying || cgs.clientNum != cg.snapshot->playerState.clientNum)
		return;

	// Go through all the active entities
	for (i = 0; i < cg.numActiveEntities; i++){
		entity = cg.activeEntities[i];

		// Calculate position and orientation
		CG_CalcPositionAndOrientation(entity);

		// If it should be ignored for anything other than binding
		if (entity->state->flags & EF_IGNORE)
			continue;

		// Set up clipping
	}
}


/*
 ==============================================================================

 PLAYER STATE PREDICTION

 ==============================================================================
*/


/*
 ==================

 Adjusts position and orientation for movement of the ground entity
 ==================
*/
static void CG_AdjustForMover (playerState_t *playerState, bool thisFrame, glqVec3 &adjusted){

	trace_t		trace;
	entity_t	*groundEntity;
 	glqVec3		currentOrigin, moveOrigin;
	glqAngles	currentAngles, moveAngles;
	glqVec3		delta, move;

	adjusted = playerState->origin;

	// Get the ground entity
	if (playerState->groundEntityNum < 0 || playerState->groundEntityNum >= ENTITYNUM_MAX_NORMAL)
		return;

	groundEntity = &cgs.entities[playerState->groundEntityNum];

	if (!groundEntity->state)
		return;		// Not supposed to happen!

	// Get the current position and orientation
	if (thisFrame){
		currentOrigin = groundEntity->lerpOrigin;
		currentAngles = groundEntity->lerpAngles;
	}
	else {
		currentOrigin = groundEntity->lastLerpOrigin;
		currentAngles = groundEntity->lastLerpAngles;
	}

	// If it didn't move at all, don't bother with it
	if (groundEntity->snapshotOrigin == currentOrigin && groundEntity->snapshotAngles == currentAngles)
		return;

	// Determine total movement of the ground entity
	moveOrigin = currentOrigin - groundEntity->snapshotOrigin;
	moveAngles = currentAngles - groundEntity->snapshotAngles;

	moveAngles.Normalize180();

	// Calculate movement
	delta = playerState->origin - groundEntity->lerpOrigin;

	move = moveOrigin + (moveAngles.ToMat3() * delta) - delta;

	// Try moving the player

	// Add movement
	adjusted += move * trace.fraction;

	if (thisFrame)
		return;

	playerState->deltaAngles.yaw += moveAngles.yaw * trace.fraction;
}

/*
 ==================
 CG_InterpolatePlayerState

 Generates cg.predictedPlayerState by interpolating between
 cg.snapshot->playerState and cg.nextSnapshot->playerState
 ==================
*/
static void CG_InterpolatePlayerState (){

	playerState_t	*playerState, *nextPlayerState;

	glqMemory->Copy(&cg.predictedPlayerState, &cg.snapshot->playerState, sizeof(playerState_t));

	// Interpolate if possible
	if (!cg.frameInterpolation || cg.nextFrameTeleport)
		return;

	playerState = &cg.snapshot->playerState;
	nextPlayerState = &cg.nextSnapshot->playerState;

	cg.predictedPlayerState.origin.LerpFast(playerState->origin, nextPlayerState->origin, cg.frameInterpolation);
	cg.predictedPlayerState.velocity.LerpFast(playerState->velocity, nextPlayerState->velocity, cg.frameInterpolation);
	cg.predictedPlayerState.viewAngles.LerpFast(playerState->viewAngles, nextPlayerState->viewAngles, cg.frameInterpolation);
}

/*
 ==================
 CG_PredictPlayerState

 Generates cg.predictedPlayerState for the current cg.time.

 For demo playback, this will be an interpolation between two valid player
 states.

 For normal gameplay, it will be the result of predicted user commands on top
 of the current player state received from the server.

 Each new snapshot will usually have one or more new user commands over the
 last, but we simulate all unacknowledged commands each time, not just the new
 ones. This means that on an internet connection, quite a few moves may be
 issued each frame.

 We detect prediction errors and allow them to be decayed off over several
 frames to ease the jerk.

 OPTIMIZE: don't re-simulate unless the newly arrived snapshot's player state
 differs from the predicted one

 TODO: set more physics stuff later if needed
 ==================
*/
void CG_PredictPlayerState (){

	etPhysicsPlayer	physics;
	playerState_t	oldPlayerState;
	userCmd_t		oldestUserCmd;
	int				userCmdNumber;
	glqVec3			delta, adjusted;
	float			length, frac;
	bool			moved = false;
	int				i;

	// If this is the first frame, we must guarantee cg.predictedPlayerState is
	// valid even if there is some other error condition
	if (!cg.validPPS){
		cg.validPPS = true;

		glqMemory->Copy(&cg.predictedPlayerState, &cg.snapshot->playerState, sizeof(playerState_t));
	}

	// If not predicting local movement, just interpolate
	if (cg_skipPrediction->integerValue || cgs.demoPlaying || cgs.clientNum != cg.snapshot->playerState.clientNum){
		CG_InterpolatePlayerState();
		return;
	}

	// If we don't have the user commands right after the snapshot, we can't
	// accurately predict a current position, so just freeze at the last good
	// position we had
	userCmdNumber = etClient->GetUserCmdNumber();

	etClient->GetUserCmd(userCmdNumber - USERCMD_BACKUP + 1, &oldestUserCmd);

	if (oldestUserCmd.time > cg.snapshot->playerState.userCmdTime){
		if (cg_showPredictionMiss->integerValue)
			G_Printf("Exceeded USERCMD_BACKUP\n");

		return;
	}

	// Save the player state before the move so we can detect transitions
	glqMemory->Copy(&oldPlayerState, &cg.predictedPlayerState, sizeof(playerState_t));

	// Get the player state from the current snapshot
	glqMemory->Copy(&cg.predictedPlayerState, &cg.snapshot->playerState, sizeof(playerState_t));

	// Prepare for a move
	physics.state = &cg.predictedPlayerState;

	physics.debug = cg_debugMove->integerValue;

	physics.gravity = cgs.gravity;

	// Execute user commands
	for (i = userCmdNumber - USERCMD_BACKUP + 1; i <= userCmdNumber; i++){
		// Get the user command
		etClient->GetUserCmd(i, &physics.userCmd);

		// Don't execute if this is an old user command which is already
		// executed
		if (physics.userCmd.time <= cg.predictedPlayerState.userCmdTime)
			continue;

		// Check for a prediction error from last frame.
		// On a LAN, this will often be the exact value from the snapshot, but
		// on a WAN we will have to predict several user commands to get to the
		// point we want to compare.
		if (cg.predictedPlayerState.userCmdTime == oldPlayerState.userCmdTime){
			if (cg.thisFrameTeleport){
				cg.thisFrameTeleport = false;

				// A teleport will not cause an error decay
				cg.predictedError.Clear();

				if (cg_showPredictionMiss->integerValue)
					G_Printf("Prediction teleport\n");
			}
			else {
				CG_AdjustForMover(&cg.predictedPlayerState, false, adjusted);

				if (cg_showPredictionMiss->integerValue){
					if (adjusted != oldPlayerState.origin)
						G_Printf("Prediction error\n");
				}

				delta = oldPlayerState.origin - adjusted;
				length = delta.Length();

				if (length > 0.1f){
					if (cg_showPredictionMiss->integerValue)
						G_Printf("Prediction miss: %f\n", length);

					if (!cg_predictionErrorDecay->integerValue)
						cg.predictedError.Clear();
					else {
						frac = (float)(cg_predictionErrorDecay->integerValue - (cg.time - cg.predictedErrorTime)) / cg_predictionErrorDecay->integerValue;
						if (frac < 0.0f)
							frac = 0.0f;

						if (cg_showPredictionMiss->integerValue){
							if (frac > 0.0f)
								G_Printf("Double prediction decay: %f\n", frac);
						}

						cg.predictedError *= frac;
					}

					cg.predictedError += delta;
					cg.predictedErrorTime = cg.oldTime;

					if (cg_showPredictionMiss->integerValue)
						G_Printf("Total predicted error: %f\n", cg.predictedError.Length());
				}
			}
		}

		// Move the player
		moved = true;

		physics.MovePlayer();
	}

	if (!moved){
		if (cg_showPredictionMiss->integerValue)
			G_Printf("Not moved\n");

		return;
	}

	// Adjust position and orientation for movement of the ground entity
	CG_AdjustForMover(&cg.predictedPlayerState, true, cg.predictedPlayerState.origin);

	// Transition player state
	CG_TransitionPlayerState(&oldPlayerState, &cg.predictedPlayerState);
}